#include <stdlib.h>
#include "dijkstra.h"

DijkstraAlgorithm::DijkstraAlgorithm(std::vector< GNode > * adjacencyMatrix, int* vertexId, int size)
{
    this->ady_ = adjacencyMatrix;
    this->vertexId_ = vertexId;
    this->size_ = size;
    initArrays();
}

DijkstraAlgorithm::~DijkstraAlgorithm()
{
    if (this->distance_)
        delete[] this->distance_;
    if (this->visited_)
        delete[]this->visited_;
    if (this->previous_)
        delete[] this->previous_;
    if (this->ady_)
        delete[] this->ady_;
    if (this->vertexId_)
        delete[] this->vertexId_;
    this->distance_ = 0;
    this->visited_ = 0;
    this->previous_ = 0;
    this->ady_ = 0;
    this->vertexId_ = 0;
}

void DijkstraAlgorithm::initArrays()
{
    this->distance_ = new int[size_];
    this->visited_ = new bool[size_];
    this->previous_ = new int[size_];
    for (int i = 0; i < size_; ++i)
    {
        this->distance_[i] = INF;
        this->visited_[i] = false;
        this->previous_[i] = -1;
    }
}

std::vector<int> DijkstraAlgorithm::shortestPath(int beginId, int endId)
{
    int actual;
    int adyPos;
    int weight;
    int begin = getPosById(beginId);
    this->queue_.push(GNode(begin, 0));
    distance_[begin] = 0;
    while (!this->queue_.empty())
    {
        actual = this->queue_.top().first;
        this->queue_.pop();
        if (visited_[actual]) continue;
        else
        {
            visited_[actual] = true;
            for (int i = 0; i<this->ady_[actual].size(); ++i)
            {
                adyPos = ady_[ actual ][ i ].first;
                weight = ady_[ actual ][ i ].second;
                relax(actual, adyPos, weight);
            }
        }
    }
    return getShortestPathIds(endId);
}


void DijkstraAlgorithm::relax(int actual, int adyPos, int weight)
{
    if (distance_[ actual ] + weight < distance_[ adyPos ])
    {
        distance_[ adyPos ] = distance_[ actual ] + weight;
        previous_[ adyPos ] = actual;
        this->queue_.push(GNode(adyPos, distance_[ adyPos ]));
    }
}

int DijkstraAlgorithm::getPosById(int id)
{
    for (int i = 0; i<this->size_; i++)
    {
        if (this->vertexId_[i] == id)
        {
            return i;
        }
    }
    return 0;
}

std::vector<int> DijkstraAlgorithm::getShortestPathIds(int endId)
{
    std::vector<int> shortestPathIds;
    int endPos = getPosById(endId);
    shortestPathIds.push_back(this->vertexId_[endPos]);
    while (previous_[endPos] != -1)
    {
        shortestPathIds.push_back(this->vertexId_[previous_[endPos]]);
        endPos = previous_[endPos];
    }
    return shortestPathIds;
}

std::vector<int> DijkstraAlgorithm::shortestPathExcludePoints(int beginId, int endId,std::vector<int> avoidPointsId)
{
    for (int i = 0; i < avoidPointsId.size(); i++)
    {
        this->visited_[getPosById(avoidPointsId[i])]=true;
    }
    return shortestPath(beginId,endId);
}

